/***************************************************************************\
 * Copyright (C) by Keio University
 * OpenNIDepthInputStream.cpp created in 10 2011.
 * Mail : fdesorbi@hvrl.ics.keio.ac.jp
 *
 * OpenNIDepthInputStream.cpp is part of the HVRL Engine Project.
 *
 * The HVRL Engine Project is free software; you can redistribute it and/or modify
 * it under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation; either version 3 of the License, or
 * (at your option) any later version.
 *
 * The HVRL Engine Project is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 \***************************************************************************/

#include "hvrl/video/OpenNIDepthInputStream.hpp"
#include "hvrl/common/Common.hpp"

namespace hvrl
{

#define MM_TO_METER 0.001f

  OpenNIDepthInputStream::OpenNIDepthInputStream(void) :
      depth_size(XN_VGA_X_RES, XN_VGA_Y_RES), color_size(XN_VGA_X_RES,
          XN_VGA_Y_RES), ir_size(XN_VGA_X_RES,
              XN_VGA_Y_RES)
  {

 /*   xnLogSetConsoleOutput(false);
    xnLogSetMaskMinSeverity("OpenNIDepthInputStream", XN_LOG_SEVERITY_NONE);
    if (Log::getInstance().getLevel() == COMPLETE
        || Log::getInstance().getLevel() == DEBUG_MODE)
      {
        xnLogSetConsoleOutput(true);
        xnLogSetMaskMinSeverity("OpenNIDepthInputStream", XN_LOG_VERBOSE);
      }
    else
      {
        if (Log::getInstance().getLevel() == WARNING)
          {
            xnLogSetFileOutput(true);
            xnLogSetMaskMinSeverity("OpenNIDepthInputStream", XN_LOG_WARNING);
          }
        else
          {
            if (Log::getInstance().getLevel() == ERROR)
              {
                xnLogSetFileOutput(true);
                xnLogSetMaskMinSeverity("OpenNIDepthInputStream", XN_LOG_ERROR);
              }
          }
      }
    xnLogInitSystem();

    XnStatus nRetVal = this->context.Init();

    if (nRetVal != XN_STATUS_OK)
      {
        throw(Exception("OpenNIDepthInputStream::OpenNIDepthInputStream",
            std::string("Failed initializing data: ")
                + xnGetStatusString(nRetVal)));
      }

    XnLicense m_license;
    strcpy(m_license.strVendor, "PrimeSense");
    strcpy(m_license.strKey, "0KOIk2JeIBYClPWVnMoRKn5cdY4=");
    this->context.AddLicense(m_license);

    xn::NodeInfoList device_node_info_list;
    nRetVal = this->context.EnumerateProductionTrees(XN_NODE_TYPE_DEVICE, NULL, device_node_info_list);
    if (nRetVal != XN_STATUS_OK)
        {
          throw(Exception("OpenNIDepthInputStream::OpenNIDepthInputStream",
              std::string("Enumerating devices failed: ")
                  + xnGetStatusString(nRetVal)));
        }
    xn::NodeInfoList::Iterator nodeIt = device_node_info_list.Begin();
    xn::NodeInfo deviceInfo = *nodeIt;

    nRetVal = this->context.CreateProductionTree(deviceInfo, device);
    if (nRetVal != XN_STATUS_OK)
        {
          throw(Exception("OpenNIDepthInputStream::OpenNIDepthInputStream",
              std::string("Creation of production tree failed: ")
                  + xnGetStatusString(nRetVal)));
        }

    nRetVal = this->depth.Create(context);
    if (nRetVal != XN_STATUS_OK)
      {
        throw(Exception("OpenNIDepthInputStream::OpenNIDepthInputStream",
            std::string("Failed creating depth: ") + xnGetStatusString(nRetVal)));
      }

    XnMapOutputMode outputMode;
    outputMode.nXRes = this->depth_size.width;
    outputMode.nYRes = this->depth_size.height;
    outputMode.nFPS = 30;

    this->depth.SetMapOutputMode(outputMode);
    this->depth.GetMetaData(this->depth_metadata);
    this->depth_size.width = this->depth_metadata.FullXRes();
    this->depth_size.height = this->depth_metadata.FullYRes();
    this->depth.StartGenerating();

    nRetVal = this->color.Create(context);
    if (nRetVal != XN_STATUS_OK)
      {
        throw(Exception("OpenNIDepthInputStream::OpenNIDepthInputStream",
            std::string("Failed creating color: ") + xnGetStatusString(nRetVal)));
      }

    outputMode.nXRes = this->color_size.width;
    outputMode.nYRes = this->color_size.height;
    this->color.SetMapOutputMode(outputMode);
    this->color.GetMetaData(this->color_metadata);
    this->color_size.width = this->color_metadata.FullXRes();
    this->color_size.height = this->color_metadata.FullYRes();
    this->color.StartGenerating();

    nRetVal = this->ir.Create(context);
    if (nRetVal != XN_STATUS_OK)
      {
        throw(Exception("OpenNIDepthInputStream::OpenNIDepthInputStream",
            std::string("Failed creating ir: ") + xnGetStatusString(nRetVal)));
      }

    if (this->depth.IsCapabilitySupported(XN_CAPABILITY_FRAME_SYNC))
      {
        this->depth.GetFrameSyncCap().FrameSyncWith(this->color);
      }

    if (this->depth.IsCapabilitySupported(XN_CAPABILITY_ALTERNATIVE_VIEW_POINT))
      {
        this->depth.GetAlternativeViewPointCap().SetViewPoint(this->color);
      }
    this->color.GetAlternativeViewPointCap().SetViewPoint(this->color);

    XnUInt64 F;
    XnDouble pixel_size;

    // get the focal length in mm (ZPD = zero plane distance)
    nRetVal = this->depth.GetIntProperty("ZPD", F);
    if (nRetVal != XN_STATUS_OK)
      {
        throw(Exception("OpenNIDepthInputStream::OpenNIDepthInputStream",
            std::string("Failed getting parameter: ")
                + xnGetStatusString(nRetVal)));
      }

    // get the pixel size in mm ("ZPPS" = pixel size at zero plane)
    nRetVal = this->depth.GetRealProperty("ZPPS", pixel_size);
    if (nRetVal != XN_STATUS_OK)
      {
        throw(Exception("OpenNIDepthInputStream::OpenNIDepthInputStream",
            std::string("Failed getting parameter: ")
                + xnGetStatusString(nRetVal)));
      }

    // normal size comes from SXGA resolution
    pixel_size *= 2.0;

    // Compute the back-projection parameters assuming pixels are squared
    this->bpcoeff = pixel_size / float(F);*/
  }

 bool OpenNIDepthInputStream::init(/*xn::NodeInfo& deviceNode, xn::Context& g_Context*/){
    XnStatus nRetVal = XN_STATUS_OK;

     XnMapOutputMode outputMode;
     outputMode.nXRes = this->depth_size.width;
     outputMode.nYRes = this->depth_size.height;
     outputMode.nFPS = 30;

     this->depth.SetMapOutputMode(outputMode);
     this->depth.GetMetaData(this->depth_metadata);
     this->depth_size.width = this->depth_metadata.FullXRes();
     this->depth_size.height = this->depth_metadata.FullYRes();
     outputMode.nXRes = this->color_size.width;
     outputMode.nYRes = this->color_size.height;
     this->color.SetMapOutputMode(outputMode);
     this->color.GetMetaData(this->color_metadata);
     this->color_size.width = this->color_metadata.FullXRes();
     this->color_size.height = this->color_metadata.FullYRes();

    if (this->depth.IsCapabilitySupported(XN_CAPABILITY_ALTERNATIVE_VIEW_POINT))
      {
        this->depth.GetAlternativeViewPointCap().SetViewPoint(this->color);
      }
    this->color.GetAlternativeViewPointCap().SetViewPoint(this->color);
    XnUInt64 F;
    XnDouble pixel_size;

    // get the focal length in mm (ZPD = zero plane distance)
    nRetVal = this->depth.GetIntProperty("ZPD", F);
    if (nRetVal != XN_STATUS_OK)
      {
        throw(Exception("OpenNIDepthInputStream::OpenNIDepthInputStream",
            std::string("Failed getting parameter: ")
                + xnGetStatusString(nRetVal)));
      }

    // get the pixel size in mm ("ZPPS" = pixel size at zero plane)
    nRetVal = this->depth.GetRealProperty("ZPPS", pixel_size);
    if (nRetVal != XN_STATUS_OK)
      {
        throw(Exception("OpenNIDepthInputStream::OpenNIDepthInputStream",
            std::string("Failed getting parameter: ")
                + xnGetStatusString(nRetVal)));
      }

    // normal size comes from SXGA resolution
    pixel_size *= 2.0;

    // Compute the back-projection parameters assuming pixels are squared
    this->bpcoeff = pixel_size / float(F);


    this->depth.StartGenerating();
    this->color.StartGenerating();

    update();


    return true;
  }

  OpenNIDepthInputStream::~OpenNIDepthInputStream(void)
  {
   /* this->depth.StopGenerating();
    this->color.StopGenerating();
    this->depth.Release();
    this->color.Release();

    this->device.Release();

    this->context.StopGeneratingAll();
    this->context.Release();*/

  }

  const short unsigned int *
  OpenNIDepthInputStream::getDepth(void) const
  {
    return this->depth.GetDepthMap();
  }

  const unsigned char *
  OpenNIDepthInputStream::getColor(void) const
  {
    return this->color.GetImageMap();
  }

  const short unsigned int *
  OpenNIDepthInputStream::getIR(void) const
  {
    return this->ir.GetIRMap();
  }

  /*
   void convertXnPoint3DToFloat(const XnPoint3D& a, float b[3])
   {
   b[0] = a.X*0.001f;
   b[1] = a.Y*0.001f;
   b[2] = a.Z*0.001f;
   }


   const float* OpenNIDepthInputStream::get3DMap(void) const
   {
   return ddd_map;
   }*/

  void
  OpenNIDepthInputStream::update(void)
  {
    if(depth.IsNewDataAvailable()){
      depth.WaitAndUpdateData();
    }
    if(color.IsNewDataAvailable()){
      color.WaitAndUpdateData();
    }
   // XnStatus nRetVal = this->context.WaitAnyUpdateAll();

    /*   if (nRetVal == XN_STATUS_OK)
     {
     const XnDepthPixel * pDepth = this->depth.GetDepthMap();
     memcpy(this->depth_map.data,pDepth,this->depth_size.width*this->depth_size.height*sizeof(unsigned short));
     memcpy(this->color_map.data,this->color.GetImageMap(),this->color_size.width*this->color_size.height*sizeof(unsigned char)*3);

     /* unsigned int uPointNum = depth_size.width * depth_size.height;
     memset(ddd_map,0,uPointNum*3*sizeof(float));

     XnPoint3D * pDepthPointSet = new XnPoint3D [uPointNum];
     unsigned int i, j, idxShift, idx;
     for (j = 0; j <depth_size.height ; ++j)
     {
     idxShift = j * depth_size.width;
     for (i = 0; i < depth_size.width; ++i)
     {
     idx = idxShift + i;
     pDepthPointSet [idx]. X = i;
     pDepthPointSet [idx]. Y = j;
     pDepthPointSet [idx]. Z = pDepth [idx];
     }
     }

     XnPoint3D * p3DPointSet = new XnPoint3D [uPointNum];
     depth.ConvertProjectiveToRealWorld (uPointNum, pDepthPointSet, p3DPointSet);

     delete [] pDepthPointSet;

     for (i = 0; i <uPointNum; ++i)
     {
     if (p3DPointSet [i]. Z == 0)
     {
     ddd_map[i*3+2] = -1000;
     continue;
     }

     convertXnPoint3DToFloat(p3DPointSet[i], &(ddd_map[i*3]));
     }
     delete [] p3DPointSet;
     }*/

  }

  double
  OpenNIDepthInputStream::getParameter(const int&)
  {
    return 0.0;
  }
  bool
  OpenNIDepthInputStream::setParameter(const int&, const double& value)
  {
    return true;
  }

}
